/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : queueMessage.h
  * Description        :
  ******************************************************************************
  * Function           : 定义队列传输的数据结构，如有新的数据传输需要，请在该文件中添加，然后在queueCreat()
  * 					 中创建队列来存放数据
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Oct 19, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __QUEUEMESSAGE_HPP
#define __QUEUEMESSAGE_HPP


#include "main.h"
#include <stdbool.h>
#include "userlib/userlib.hpp"

//#include <Eigen>
//
//using namespace std;
//using namespace Eigen;
#ifdef __cplusplus
extern "C" {
#endif


typedef enum
{
	RC_NONE=0xFFU,    //无行为
	RC_FUN0=0x00U,    //右下 右下
	RC_MOT_UNLOCK=0x01U,  //右下 左下  解锁
	RC_FUN2=0x02U,    //右上 右下
	RC_FUN3=0x03U,    //右上 左下
	RC_FUN4=0x04U,    //右下 右上
	RC_FUN5=0x05U,    //右下 左上
	RC_FUN6=0x06U,    //右上 右上
	RC_FUN7=0x07U,    //右上 左上
	RC_MOT_LOCK=0x08U,    //左下 右下  上锁
	RC_FUN9=0x09U,    //左下 左下
	RC_FUN10=0x0AU,   //左上 右下
	RC_FUN11=0x0BU,   //左上 左下
	RC_FUN12=0x0CU,   //左下 右上
	RC_FUN13=0x0DU,   //左下 左上
	RC_FUN14=0x0EU,   //左上 右上
	RC_FUN15=0x0FU,   //左上 左上
}eRC_MODE;

typedef enum
{
	MODE_MANUAL = 0x00U,          //手控
	MODE_ALTITUDE  = 0x01U,       //姿态
	MODE_HIGH_HOLD = 0x02U,       //定高
	MODE_POSITION_HOLD = 0x03U,   //定点
	MODE_TARGET = 0x04U,          //航线
	MODE_SPEED = 0x05U,          //航线
	MODE_LOST = 0x06U,          //航线
}eMODE;

typedef enum			//电量状态
{
	BATTARY_NORM = 0x00U,
	BATTARY_LOW = 0x01U,
	BATTARY_CRITICAL = 0x02U,
}eBATTARY_STA;
typedef enum {NORMAL=0,CLOSE=1,LOST=2}eRC;

/* 陀螺仪传感器原始数据 */
typedef struct
{
	float timestamp;		//time since Tim2 start(unit:s)

	float dt;				//delta time between samples(unit:s)

	float gyro[3];			//angular velocity body axis in rad/s

	float temperature;		//temperature in degrees celsius


}sensor_gyro_msg;
/* 陀螺仪传感器原始数据 */

/* 陀螺仪传感器滤波后的数据 */
typedef struct
{
	float gyro_filter[3];			//angular velocity body axis in rad/s

}sensor_gyro_filter_msg;
/* 陀螺仪传感器滤波后的数据 */

/* 加速度计传感器原始数据 */
typedef struct
{
	float timestamp;		//time since Tim2 start(unit:s)

	uint32_t timestamp_us;

	float dt;				//delta time between samples(unit:s)

	float acc[3];			//Acceleration body axis in m/s2

}sensor_acc_msg;
/* 加速度计传感器原始数据 */

/* 加速度传感器滤波后的数据 */
typedef struct
{
	float acc_filter[3];			//angular velocity body axis in rad/s
}sensor_acc_filter_msg;

/* imu向下采样，  For eskf only */
typedef struct
{
	uint32_t timestamp;		//time since Tim2 start(unit:us)
	float gyro[3];
	float acc[3];
	int* sample_cnt; // 下采样区间内imu的采样总数，eskf读取后需由eskf重置
}downsample_imu_for_eskf_msg;
/* imu向下采样 */

typedef struct
{
	float delta_T;
	float delta_vel_m[3];
	float delta_ang_m[3];
	uint32_t time_us;
}delta_angle_and_vel_msg;

typedef struct
{
	uint8_t delta_m_to_zeros_flag;
}delta_to_zeros_flag_msg;
/* 加速度传感器滤波后的数据 */


/* 磁力计传感器原始数据 */
typedef struct
{
	float timestamp;		//time since Tim2 start(unit:s)

	float dt;				//delta time between samples(unit:s)

	float mag[3];			//Magnetometer body axis in gauss
	float magR[3];			//Magnetometer body axis in gauss

}sensor_mag_msg;
/* 磁力计传感器原始数据 */

typedef struct
{
	uint32_t timestamp;		//时间戳,单位us
	uint8_t status;
	uint8_t star;			//星数
	bool isReady;
	double	lat;			//单位°
	double	lng;			//单位°
	double alti;			//高度,单位m
	double Zero_LLH[3];		//零点LLH，单位°或m
	float NED[3];			//北东地坐标，单位m
	double NED_spd[3];
	float gpsSpdAccuracy;
	float gpsPosAccuracy;
	float gpsHeiAccuracy;
}gps_msg;		//gps数据

typedef struct
{
	uint32_t timestamp;		//时间戳,单位us
	float MagRaw[3];
	float MagRel[3];  	//实际值
}mag_msg;		//磁力计数据

/* 姿态角度 ESKF */
typedef struct
{
	float Ang[3];		//姿态角, 单位rad

	float pqr[3];		//卡尔曼滤波得到的角速度, 单位rad/s

	float pqr_err[3];

	float q[4];		//卡尔曼滤波得到的角速度, 单位rad/s

//	Matrix<float,3,3> drotation;  //旋转矩阵
//
	float rotation[3][3];


	float Euler[3];

}ahrs_euler_msg;
/* 姿态角度 ESKF */

/* 电池电压  */
typedef struct
{
	uint8_t battery_n_S;		//n S电池
	float battery_Voltage;		//电池电压，伏
}battery_msg;

/* 电池电压  */

typedef struct
{
	float kp[18];
	float ki[18];
	float kd[18];
}PID_msg;

typedef struct
{
	uint32_t timestamp;		//时间戳,单位us
	float i;			//x轴光流速度，单位cm/s
	float mi;			//y轴光流速度，单位cm/s
	float rpm;			//x轴光流位置，单位cm
	float qi;			//y轴光流位置，单位cm
	float V;
	float tem;
}flow_msg;			//光流数据

typedef struct
{
	uint32_t timestamp;		//时间戳,单位us
	float flowx;			//x轴光流速度，单位cm/s
	float flowy;			//y轴光流速度，单位cm/s
	float flowx2;			//x轴光流位置，单位cm
	float flowy2;			//y轴光流位置，单位cm
}lidar_msg;			//光流数据

typedef struct
{
	uint32_t timestamp;		//时间戳,单位us
	uint32_t cyctime;
	float MainRotorSpeed;
	float height;			//高度,单位dm
	float speedZ;			//高度,单位dm/s
}height_msg;		//激光高度数据


/* 遥控器数据 */
typedef struct
{
	uint16_t  PPM[16];		//遥控器各通道PPM
}RCData;
/* 遥控器数据 */
typedef struct
{
	eRC rc_status;
	bool ever_rc;
	int rtcm_id[5];
	int rctm_id_cnt;
}RC_Status_msg;

/* 遥控器数据 */
typedef struct
{
	float Val[4];     //roll pitch yaw high
	uint8_t    Key[4];     //4个开关
	float Ang[3];     //遥控器角度 roll pitch YAW
	float dAng;       //角速度
	float Thr;        //遥控器油门
	float Uvw[3];     //遥控器Uvw
	uint16_t  PPM[16];
	eRC_MODE  Mode;   //行为模式

	bool OneKeyTakeoff;		//1为一键起飞模式
	bool Frist_Entry_TakeOff;
	bool TakeOffFinish;//一键起降待机模式 该值为0时锁电机，为1时解锁

	bool OneKeyLanding;		//1为一键降落模式

	bool LandingFinish;//一键起降待机模式 该值为0时锁电机，为1时解锁

	int TakeOff_Trajectory_Landing;//0为地面待机+起飞，1为轨迹模式，2为返航过程，3为降落过程

}RC_command_msg;
/* 遥控器数据 */

typedef struct
{
	double output[5];//控制输出,T,Mx,My,Mz,β
	int cs_output[7];//舵面输出
	int mt_output[2];//电机输出
	float debug_pid[3];
	eMODE control_mode;
}Control_output_msg;

typedef struct
{
	uint16_t PWM_OBS[8];
	float MainRotorSpeed;
	float MainRotorSpeed_Fil;
}Motor_msg;


typedef struct
{
	uint32_t timestamp;		//时间戳,单位us
	float altitude;			//高度,单位m
	float altSlope;			//高度,单位m/s
}sensor_baroAlt_msg;		//激光高度数据


typedef struct
{
	float posCommand[3];
	float posFeedback[3];
	float velCommand[3];
	float velFeedback[3];
	float angFromRCorOutloop[3];
	float angCommand[3];
	float angRateCommand[4];
	float accCommand[3];
	float other[5];
	float auto_servo_width[8];
}control_transfer_msg;

typedef struct
{
	double Ned[3];
	double Ned_spd[3];
	double Ned_a[3];
	double Acc_bias[3];
}ekf_cplfil_msg;		//EKF互补滤波输出

typedef struct
{
//	float q[4];					//四元数
	float rotation[3][3];
	float Attitude[3];			//欧拉角,单位（弧度radian），roll,pitch,yaw
	float Pos[3];				//卡尔曼北东地坐标，单位（弧度radian）
	float Ned_spd[3];
	float Gyro_bias[3];
	float Acc_bias[3];

//	float pos_cor[3];
//	float vel_cor[3];
//	float ang_cor[3];
//	eBODY_FRAME bodyFrame;

	float delta_Laser_and_RTK;
}eskf_msg;

typedef struct
{
	float TargetPointList[MAX_QUEUE_SIZE][4]; //
	float yawCommandList[MAX_QUEUE_SIZE];
	int fptr; //队头指针
	int eptr; //队尾指针
	bool isUpdate; //更新标志位
}targetPoint_msg;

typedef struct
{
	float yget[2];
	float xdot[3];
	float u;
	int realu;
}MPC_msg;		//EKF互补滤波输出

typedef struct
{
	float TD_state[2];
	float ESO_state[3];
	float target_rate;
	float yaw_out;
}ADRC_msg; //ADRC控制器

#ifdef __cplusplus
}
#endif

#endif

/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
